/**
  ******************************************************************************
  * @file           : communicate_task.cpp
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/2/15
  ******************************************************************************
  */



#include "communicate_task.h"




rc_info rc_msg_comm={0};
extern rc_device_t g_rc;
extern osSemaphoreId_t LeftUSBDataUpdateBinarySemHandle;
extern osSemaphoreId_t RightUSBDataUpdateBinarySemHandle;
extern osSemaphoreId_t VisionCalBinarySemHandle;


extern GyroDataStruct GyroDataSave;
extern StampDataStruct TimeStampedData;

extern uint16_t shooter_barrel_cooling_value; //每秒冷却热量
extern uint16_t shooter_barrel_heat_limit;    //热量上限

extern AimAutoStruct AimAutoData;

extern uint8_t vision_cal_completed;
extern pose_cal poseCal;

extern uint8_t is_right_of_axis;
extern uint8_t is_temporary_pause;

extern uint32_t target_recognition_cnt;

extern uint8_t armor_number_no_zero;

extern uint8_t VisionControlFlag;
extern AutoAimingParaStruct AutoAimingPara;

extern uint8_t is_huart6_lost;
extern uint32_t huart6_cnt;

extern uint16_t judge_current_total_heat;

uint32_t uart_err_count=0;
uint16_t len_buff=0;
uint32_t CameraReceiveCount = 0;
float CameraReceiveDelta = 0;
chassis_info chassisInfo;

judge_info_0 judgeInfo0;
judge_info_1 judgeInfo1;
judge_info_2 judgeInfo2;
judge_info_3 judgeInfo3;


twins_receive_info twins_receive;
twins_receive_info_tran twins_receive_tran;

twins_tran_info twins_tran={};

uint8_t enemy_aim_enable[12] = {0,1,0,1,1,1,1,0,1,1,1,0};

uint8_t mini_camera_0_enemy_data[12] = {};      //第0个表示是否出现敌人

void vision_transmit(){
#if DEBUG_VISION_TRANS
    if(DEBUG_VISION_FLAG){
        //Get Gyro and encoder value
        GyroAndEncoderDataGet();
        TimeStampedData.EncoderData = EncoderDataSave;
        TimeStampedData.GyroData = GyroDataSave;
        TimeStampedData.time_stamp=osKernelGetTickCount();//Increase
        time1 = TimeStampedData.time_stamp;
        //USB TxMessage
        TransmitDataTest(&VisionTransmitTest,UsbTxBuf);
        len_buff = sizeof(UsbTxBuf);
        CDC_Transmit_FS(UsbTxBuf,USB_Buff_Size);		//·???
//            HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOB,GPIO_Pin,GPIO_PIN_SET);
        DEBUG_VISION_FLAG = 0;
    }
    else{
//            HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOB,GPIO_Pin,GPIO_PIN_RESET);
        DEBUG_VISION_FLAG = 0;
    }
#else
    /*
     * 自瞄7
     *
     */
    CameraReceiveDelta = (float)(1.0f / DWT_GetDeltaT(&CameraReceiveCount));
    //Get Gyro and encoder value
    GyroDataGet();
    TimeStampedData.GyroData = GyroDataSave;
    TimeStampedData.time_stamp=osKernelGetTickCount();//Increase
    //USB TxMessage
    TransmitData(&VisionTransmit, usb_tx_buf_left);
    len_buff = sizeof(usb_tx_buf_left);
    CDC_Transmit_FS(usb_tx_buf_left, USB_Buff_Size);
    osDelay(2);

    uart_err_count++;
    if(uart_err_count > 5) {
        usb_tx_buf_right[0] = 0x80;
    }
    CDC_Transmit_FS(usb_tx_buf_right, USB_Buff_Size);
    osDelay(2);
#endif
}

void can_chassis_callback(can_device_receive *can_receive, uint8_t *data){
    auto tmp = (chassis_info_tran *)data;
    chassisInfo.yaw_angle = tmp->yaw_angle; //已归一化
    chassisInfo.is_flip_over = tmp->is_flip_over;
}

/**
 * @param can_receive
 * @param data
 * 这里下面的两个赋值是因为main.c中已经重定义了回调函数，所以需要复用以下
 */
void can_judge0_callback(can_device_receive *can_receive, uint8_t *data){
    judge_info_0_tran *judgeInfo0Tran = (judge_info_0_tran *)data;

    judgeInfo0.mains_power_shooter_output = judgeInfo0Tran->mains_power_shooter_output;
    judgeInfo0.HP_deduction_reason = judgeInfo0Tran->HP_deduction_reason;
    judgeInfo0.be_hit = judgeInfo0Tran->be_hit;
    judgeInfo0.game_progress = judgeInfo0Tran->game_progress;
    judgeInfo0.shooter_barrel_cooling_value = judgeInfo0Tran->shooter_barrel_cooling_value;
    judgeInfo0.stage_remain_time = judgeInfo0Tran->stage_remain_time;
    judgeInfo0.last_HP = judgeInfo0.current_HP;
    judgeInfo0.current_HP = judgeInfo0Tran->current_HP;
    judgeInfo0.hurt_armor_id = judgeInfo0Tran->hurt_armor_id;
    judgeInfo0.is_occupy_patrol_zone = judgeInfo0Tran->is_occupy_patrol_zone;
    judgeInfo0.is_occupy_healing_spot = judgeInfo0Tran->is_occupy_healing_spot;

    shooter_barrel_cooling_value = judgeInfo0.shooter_barrel_cooling_value;
}

void can_judge1_callback(can_device_receive *can_receive, uint8_t *data){
    judge_info_1_tran *judgeInfo1Tran = (judge_info_1_tran *)data;

    judgeInfo1.game_type = judgeInfo1Tran->game_type;
    judgeInfo1.shooter_barrel_heat_limit = judgeInfo1Tran->shooter_barrel_heat_limit;
    judgeInfo1.is_blue = judgeInfo1Tran->is_blue;
    judgeInfo1.maximum_HP = judgeInfo1Tran->maximum_HP;
    judgeInfo1.remaining_gold_coin = judgeInfo1Tran->remaining_gold_coin;

    shooter_barrel_heat_limit = judgeInfo1.shooter_barrel_heat_limit - 10;
}

void can_judge2_callback(can_device_receive *can_receive, uint8_t *data) {
    judge_info_2_tran *judgeInfo2Tran = (judge_info_2_tran *)data;

    judgeInfo2.projectile_allowance_17mm = judgeInfo2Tran->projectile_allowance_17mm;
    judgeInfo2.shooter_id = judgeInfo2Tran->shooter_id;
    judgeInfo2.bullet_speed = judgeInfo2Tran->bullet_speed;
    if(judgeInfo2.shooter_id == 1 && judgeInfo2.bullet_speed > 20){
        AutoAimingPara.bullet_actual_speed = judgeInfo2.bullet_speed;
    }

}

void can_judge3_callback(can_device_receive *can_receive, uint8_t *data) {
    judge_info_3_tran *judgeInfo3Tran = (judge_info_3_tran *)data;

    judgeInfo3.shooter_17mm_barrel_heat = judgeInfo3Tran->shooter_17mm_1_barrel_heat;
    judge_current_total_heat = judgeInfo3.shooter_17mm_barrel_heat;
}


void communicate_can_task(void *argument){
    can_device_transmit can_rc_info(&hcan2, 0x300, 8, (uint8_t *)&rc_msg_comm);
    can_device_receive can_chassis_info(&hcan2, 0x301, can_chassis_callback);
    can_device_receive can_judge_info_0(&hcan2, 0x303, can_judge0_callback);
    can_device_receive can_judge_info_1(&hcan2, 0x304, can_judge1_callback);
    can_device_receive can_judge_info_2(&hcan2, 0x30a, can_judge2_callback);
    can_device_receive can_judge_info_3(&hcan2, 0x30f, can_judge3_callback);

    for(;;){

        taskENTER_CRITICAL();
        rc_msg_comm.ch0 = g_rc.raw.ch0;
        rc_msg_comm.ch1 = g_rc.raw.ch1;
        rc_msg_comm.ch2 = g_rc.raw.ch2;
        rc_msg_comm.ch3 = g_rc.raw.ch3;
        rc_msg_comm.s1 = g_rc.raw.s1;
        rc_msg_comm.s2 = g_rc.raw.s2;
        rc_msg_comm.is_lost = g_rc.is_lost;
        taskEXIT_CRITICAL();
        can_rc_info.can_send_msg();

        osDelay(2);
    }
}


void communicate_usb_receive_task(void *argument){
    osSemaphoreAcquire(LeftUSBDataUpdateBinarySemHandle, 0);
    osSemaphoreAcquire(VisionCalBinarySemHandle, 0);
    for(;;){
        osSemaphoreAcquire(LeftUSBDataUpdateBinarySemHandle, osWaitForever);
        ReceiveData(usb_rx_buf_left);
        osSemaphoreRelease(VisionCalBinarySemHandle);
    }
}

void communicate_usb_transmit_task(void *argument){
    for(;;){
        vision_transmit();
    }
}


void communicate_uart_vision_task(void *argument){
    osSemaphoreAcquire(RightUSBDataUpdateBinarySemHandle, 0);
    osStatus_t status;
    for(;;){
        HAL_UARTEx_ReceiveToIdle_DMA(&huart1, usb_rx_buf_right, sizeof(usb_rx_buf_right));
        __HAL_DMA_DISABLE_IT(huart1.hdmarx, DMA_IT_HT);
        status = osSemaphoreAcquire(RightUSBDataUpdateBinarySemHandle, 0);
        if(status == osOK){
            HAL_UART_Transmit_DMA(&huart1, (uint8_t *)usb_rx_buf_right, sizeof(usb_rx_buf_right));
        }
        osDelay(1);
    }
}

void communicate_uart_twins_task(void *argument){
    HAL_UARTEx_ReceiveToIdle_DMA(&huart6, (uint8_t *)&twins_receive_tran, sizeof(twins_receive_tran));
    __HAL_DMA_DISABLE_IT(huart6.hdmarx, DMA_IT_HT);
    for(;;){
        HAL_UARTEx_ReceiveToIdle_DMA(&huart6, (uint8_t *)&twins_receive_tran, sizeof(twins_receive_tran));
        __HAL_DMA_DISABLE_IT(huart6.hdmarx, DMA_IT_HT);
        taskENTER_CRITICAL();
        twins_tran.frame_header = 0x30;
        twins_tran.current_left_yaw = INS.My_Yaw;
        twins_tran.is_target_detected = target_recognition_cnt > 0 ? vision_cal_completed : 0;
        twins_tran.armor_number_last = armor_number_no_zero;
        twins_tran.twins_pitch = poseCal.gimbal_pitch;
        twins_tran.twins_yaw = poseCal.gimbal_yaw;
        twins_tran.chassis_cal_yaw = poseCal.chassis_yaw;
        twins_tran.is_right_of_axis = is_right_of_axis;
        twins_tran.is_chassis_yaw_stop = is_temporary_pause || (mini_camera_0_enemy_data[0] == 1 && VisionControlFlag == 0 && AimAutoData.AutoAim == 1);
        twins_tran.frame_tail = 0x31;
        taskEXIT_CRITICAL();
        HAL_UART_Transmit_DMA(&huart6, (uint8_t *)&twins_tran, sizeof(twins_tran));

        huart6_cnt++;
        is_huart6_lost = huart6_cnt > 30 ? 1 : 0;
        osDelay(2);
    }
}








